//
// Created by Konodoki on 2024/9/14.
//

#include "SolveTrajectory.h"
// 近点只考虑水平方向的空气阻力



//TODO 完整弹道模型
//TODO 适配英雄机器人弹道解算


#include <math.h>
#include <stdio.h>

#include "SolveTrajectory.h"

struct tar_pos tar_position[4]; //最多只有四块装甲板
float t = 0.5f; // 飞行时间



/*
@brief 单方向空气阻力弹道模型
@param s:m 距离
@param v:m/s 速度
@param angle:rad 角度
@return z:m
*/
float monoDirectionalAirResistanceModel(struct SolveTrajectoryParams *st,float s, float v, float angle)
{
  float z;
  //t为给定v与angle时的飞行时间
  t = (float)((exp(st->k * s) - 1) / (st->k * v * cos(angle)));
  if(t < 0)
  {
    //由于严重超出最大射程，计算过程中浮点数溢出，导致t变成负数
    //    printf("[WRAN]: Exceeding the maximum range!\n");
    //重置t，防止下次调用会出现nan
    t = 0;
    return 0;
  }
  //z为给定v与angle时的高度
  z = (float)(v * sin(angle) * t - GRAVITY * t * t / 2);
  //  printf("model %f %f\n", t, z);
  return z;
}


/*
@brief 完整弹道模型
@param s:m 距离
@param v:m/s 速度
@param angle:rad 角度
@return z:m
*/
//TODO 完整弹道模型
float completeAirResistanceModel(struct SolveTrajectoryParams *st,float s, float v, float angle)
{

  return 0;

}



/*
@brief pitch轴解算
@param s:m 距离
@param z:m 高度
@param v:m/s
@return angle_pitch:rad
*/
float pitchTrajectoryCompensation(struct SolveTrajectoryParams *st,float s, float z, float v)
{
  float z_temp, z_actual, dz;
  float angle_pitch;
  int i = 0;
  z_temp = z;
  // iteration
  for (i = 0; i < 50; i++)
  {
    angle_pitch = atan2(z_temp, s); // rad
    z_actual = monoDirectionalAirResistanceModel(st,s, v, angle_pitch);
    if(z_actual == 0)
    {
      angle_pitch = 0;
      break;
    }
    dz = 0.3*(z - z_actual);
    z_temp = z_temp + dz;
    //    printf("iteration num %d: angle_pitch %f, temp target z:%f, err of z:%f, s:%f\n",
    //           i + 1, angle_pitch * 180 / PI, z_temp, dz,s);
    if (fabsf(dz) < 0.00001)
    {
      break;
    }
  }
  return angle_pitch;
}

/*
@brief 根据最优决策得出被击打装甲板 自动解算弹道
@param pitch:rad  传出pitch
@param yaw:rad    传出yaw
@param aim_x:传出aim_x  打击目标的x
@param aim_y:传出aim_y  打击目标的y
@param aim_z:传出aim_z  打击目标的z
*/
void autoSolveTrajectory(struct SolveTrajectoryParams *st,float *pitch, float *yaw, float *aim_x, float *aim_y, float *aim_z)
{

  // 线性预测
  float timeDelay = st->bias_time/1000.0 + t;
  st->tar_yaw += st->v_yaw * timeDelay;

  //计算四块装甲板的位置
  //装甲板id顺序，以四块装甲板为例，逆时针编号
  //      2
  //   3     1
  //      0
  int use_1 = 1;
  int i = 0;
  int idx = 0; // 选择的装甲板
  //armor_num = ARMOR_NUM_BALANCE 为平衡步兵
  if (st->armor_num == ARMOR_NUM_OUTPOST) {  //前哨站
    for (i = 0; i<3; i++) {
      float tmp_yaw = st->tar_yaw + i * 2.0 * PI/3.0;  // 2/3PI
      float r =  (st->r1 + st->r2)/2;   //理论上r1=r2 这里取个平均值
      tar_position[i].x = st->xw - r*cos(tmp_yaw);
      tar_position[i].y = st->yw - r*sin(tmp_yaw);
      tar_position[i].z = st->zw;
      tar_position[i].yaw = tmp_yaw;
    }

    //TODO 选择最优装甲板 选板逻辑你们自己写，这个一般给英雄用


  } else {

    for (i = 0; i<4; i++) {
      float tmp_yaw = st->tar_yaw + i * PI/2.0;
      float r = use_1 ? st->r1 : st->r2;
      tar_position[i].x = st->xw - r*cos(tmp_yaw);
      tar_position[i].y = st->yw - r*sin(tmp_yaw);
      tar_position[i].z = use_1 ? st->zw : st->zw + st->dz;
      tar_position[i].yaw = tmp_yaw;
      use_1 = !use_1;
    }

    //2种常见决策方案：
    //1.计算枪管到目标装甲板yaw最小的那个装甲板
    //2.计算距离最近的装甲板

    //计算距离最近的装甲板
    float dis_diff_min = sqrt(tar_position[0].x * tar_position[0].x + tar_position[0].y * tar_position[0].y);
    int idx = 0;
    for (i = 1; i<4; i++)
    {
            float temp_dis_diff = sqrt(tar_position[i].x * tar_position[0].x + tar_position[i].y * tar_position[0].y);
            if (temp_dis_diff < dis_diff_min)
            {
                    dis_diff_min = temp_dis_diff;
                    idx = i;
            }
    }


    //计算枪管到目标装甲板yaw最小的那个装甲板
//    float yaw_diff_min = fabsf(*yaw - tar_position[0].yaw);
//    for (i = 1; i<4; i++) {
//      float temp_yaw_diff = fabsf(*yaw - tar_position[i].yaw);
//      if (temp_yaw_diff < yaw_diff_min)
//      {
//        yaw_diff_min = temp_yaw_diff;
//        idx = i;
//      }
//    }

  }



  *aim_z = tar_position[idx].z + st->vzw * timeDelay;
  *aim_x = tar_position[idx].x + st->vxw * timeDelay;
  *aim_y = tar_position[idx].y + st->vyw * timeDelay;
  //这里符号给错了
  float temp_pitch = -pitchTrajectoryCompensation(st,sqrt((*aim_x) * (*aim_x) + (*aim_y) * (*aim_y)) - st->s_bias,
                                                  *aim_z + st->z_bias, st->current_v);
  if(temp_pitch)
    *pitch = temp_pitch;
  if(*aim_x || *aim_y)
    *yaw = (float)(atan2(*aim_y, *aim_x));
}

// 从坐标轴正向看向原点，逆时针方向为正
/*

int main()
{
  float aim_x = 0, aim_y = 0, aim_z = 0; // aim point 落点，传回上位机用于可视化
  float pitch = 0; //输出控制量 pitch绝对角度 弧度
  float yaw = 0;   //输出控制量 yaw绝对角度 弧度

  //定义参数
  st->k = 0.092;
  st->bullet_type =  BULLET_17;
  st->current_v = 18;
  st->current_pitch = 0;
  st->current_yaw = 0;
  st->xw = 3.0;
  // st->yw = 0.0159;
  st->yw = 0;
  // st->zw = -0.2898;
  st->zw = 1.5;

  st->vxw = 0;
  st->vyw = 0;
  st->vzw = 0;
  st->v_yaw = 0;
  st->tar_yaw = 0.09131;
  st->r1 = 0.5;
  st->r2 = 0.5;
  st->dz = 0.1;
  st->bias_time = 100;
  st->s_bias = 0.19133;
  st->z_bias = 0.21265;
  st->armor_id = ARMOR_INFANTRY3;
  st->armor_num = ARMOR_NUM_NORMAL;


  autoSolveTrajectory(&pitch, &yaw, &aim_x, &aim_y, &aim_z);
//  printf("main pitch:%f° yaw:%f° ", pitch * 180 / PI, yaw * 180 / PI);
//  printf("\npitch:%frad yaw:%frad aim_x:%f aim_y:%f aim_z:%f", pitch, yaw, aim_x, aim_y, aim_z);

  return 0;
}*/
